class Node:
	def __init__(self, position):
		self.position = position
		
# Utility methods
def NodesFromFile(nodetxt):
	import re
	
	nodes = []
	
	regex = re.compile('\\[(.*?), (.*?), (.*?)\\]')
	file = open(nodetxt)
	
	for line in file:
		groups = regex.match(line).groups()
		position = [float(groups[0]), float(groups[1]), float(groups[2])]
		nodes.append(Node(position))
		
	file.close()
	
	return nodes

def GetDistance(A, B):
	import math
	
	X = abs(A[0] - B[0])
	Y = abs(A[1] - B[1])
	Z = abs(A[2] - B[2])
	
	Ds = (X*X)+(Y*Y)+(Z*Z)
	D = math.sqrt(Ds)
	return D
	
def GetNearestNode(target, nodes):
	best = []
	
	for node in nodes:
		if best:
			d = GetDistance(target, node.position)
			if d < best[1]:
				best = [node, d]
		else:
			d = GetDistance(target, node.position)
			best = [node, d]
			
	return best[0]
	
def GetNextNode(node, list):
	try:
		ret = list[list.index(node)+1]
	except IndexError: # We've reached the end of the list, get the first node
		ret = list[0]
	return ret
	
def GetAngleToNode(ob, node):
	from math import degrees, atan2
	
	# Get the angle between the nearest node and the next one for orientation
	x = node.position[0] - ob.worldPosition[0]
	y = node.position[1] - ob.worldPosition[1]
	
	angle = degrees(atan2(x,y))
			
	return angle